{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 76,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib qt5\n",
    "import pandas as pd\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import seaborn as sns\n",
    "import time\n",
    "from PIL import Image\n",
    "from PIL import ImageDraw\n",
    "plt.style.use({'figure.figsize':(10, 10)})\n",
    "pd.set_option('max_rows', 300)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Q-Table One"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Q-table One is used to void the obstacles automatically.\n",
    "### Columns:Nearest|Near|Medium|Far\n",
    "##### Columns register the states\n",
    "### Rows:Up|Down|Turn_left_45 degree|Turn_right_45_degree\n",
    "##### Rows register the actions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 77,
   "metadata": {},
   "outputs": [],
   "source": [
    "Epsilon_start=1\n",
    "Epsilon_final=0.01\n",
    "Decay_rate=0.000001#he dacaying rate of the Epsilon, the range of the epsilon is 0.01-1, initially it is 1.\n",
    "Action_times=0 #Rigister the totality of the times of selecting actions, including the random selections and selection based on Q_Table\n",
    "Velocity_tripod=0.289*40\n",
    "Up_degree=np.array([-40,-20,0,20,40])\n",
    "Left_degree=np.array([-60,-80,-100,-120])\n",
    "Right_degree=np.array([60,80,100,120])\n",
    "Robot_radium=40\n",
    "Beta=0.9\n",
    "Alpha=0.2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 78,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Nearest(<50cm)||Near(50cm-130cm)||Medium(130cm-210cm)|Far(>210cm)\n",
    "#Safe distance=1cm\n",
    "Q_table1_states=np.array(['L0R0U0','L0R0U1','L0R0U2','L0R0U3',\n",
    "                'L0R1U0','L0R1U1','L0R1U2','L0R1U3',\n",
    "                'L0R2U0','L0R2U1','L0R2U2','L0R2U3',\n",
    "                'L0R3U0','L0R3U1','L0R3U2','L0R3U3',\n",
    "                'L1R0U0','L1R0U1','L1R0U2','L1R0U3',\n",
    "                'L1R1U0','L1R1U1','L1R1U2','L1R1U3',\n",
    "                'L1R2U0','L1R2U1','L1R2U2','L1R2U3',\n",
    "                'L1R3U0','L1R3U1','L1R3U2','L1R3U3',\n",
    "                'L2R0U0','L2R0U1','L2R0U2','L2R0U3',\n",
    "                'L2R1U0','L2R1U1','L2R1U2','L2R1U3',\n",
    "                'L2R2U0','L2R2U1','L2R2U2','L2R2U3',\n",
    "                'L2R3U0','L2R3U1','L2R3U2','L2R3U3',\n",
    "                'L3R0U0','L3R0U1','L3R0U2','L3R0U3',\n",
    "                'L3R1U0','L3R1U1','L3R1U2','L3R1U3',\n",
    "                'L3R2U0','L3R2U1','L3R2U2','L3R2U3',\n",
    "                'L3R3U0','L3R3U1','L3R3U2','L3R3U3'])\n",
    "Q_table1_actions=np.array(['Up','Down','Left_45D','Right_45D'])\n",
    "Q_table1_actions_length=len(Q_table1_actions)\n",
    "Q_table1_states_length=len(Q_table1_states)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 79,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<div>\n",
       "<style>\n",
       "    .dataframe thead tr:only-child th {\n",
       "        text-align: right;\n",
       "    }\n",
       "\n",
       "    .dataframe thead th {\n",
       "        text-align: left;\n",
       "    }\n",
       "\n",
       "    .dataframe tbody tr th {\n",
       "        vertical-align: top;\n",
       "    }\n",
       "</style>\n",
       "<table border=\"1\" class=\"dataframe\">\n",
       "  <thead>\n",
       "    <tr style=\"text-align: right;\">\n",
       "      <th></th>\n",
       "      <th>Up</th>\n",
       "      <th>Down</th>\n",
       "      <th>Left_45D</th>\n",
       "      <th>Right_45D</th>\n",
       "    </tr>\n",
       "  </thead>\n",
       "  <tbody>\n",
       "    <tr>\n",
       "      <th>L0R0U0</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L0R0U1</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L0R0U2</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L0R0U3</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L0R1U0</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "  </tbody>\n",
       "</table>\n",
       "</div>"
      ],
      "text/plain": [
       "         Up  Down  Left_45D  Right_45D\n",
       "L0R0U0  0.0   0.0       0.0        0.0\n",
       "L0R0U1  0.0   0.0       0.0        0.0\n",
       "L0R0U2  0.0   0.0       0.0        0.0\n",
       "L0R0U3  0.0   0.0       0.0        0.0\n",
       "L0R1U0  0.0   0.0       0.0        0.0"
      ]
     },
     "execution_count": 79,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Q_table_real=np.zeros((Q_table1_states_length,Q_table1_actions_length))\n",
    "Q_table_real=pd.DataFrame(Q_table_real,columns=Q_table1_actions,index=Q_table1_states)\n",
    "Q_table_real.head()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 80,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Initial_Q_Table(LengthOfActions,LengthOfStates):\n",
    "    Q_Table=np.zeros((LengthOfStates,LengthOfActions))\n",
    "    print('***********************************************************')\n",
    "    print(\"Succeed to initialize Q-Table!\")\n",
    "    print('***********************************************************')\n",
    "    return Q_Table"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 88,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Draw_map1():\n",
    "    im = Image.new(\"RGB\", size=(2000,2000),color=(0,0,0)) \n",
    "    draw = ImageDraw.Draw(im,mode='RGB') \n",
    "    draw.rectangle((40,40,1960,1960),(255,255,255),(255,255,255)) \n",
    "    draw.rectangle((1500,1000,1650,1150),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((400,700,550,850),(0,0,0), (0,0,0)) \n",
    "    draw.rectangle((200,300,350,450),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((1500,500,1550,650),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((1200,1400,1350,1550),(0,0,0), (0,0,0)) \n",
    "    draw.rectangle((700,1200,850,1350),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((300,1600,450,1750),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((100,1100,250,1250),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((1100,250,1250,400),(0,0,0),(0,0,0)) \n",
    "    draw.polygon((900, 1070,1120, 1000,1150, 1100, 1100,1090,1050, 1200),(0,0,0),(0,0,0))\n",
    "    draw.pieslice((750, 1700, 900, 1850), 0,180,(0,0,0),(0,0,0))\n",
    "    draw.ellipse((900,550,1050,700),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((650,100,750,200),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((1700,130,1800,230),(0,0,0),(0,0,0))\n",
    "    draw.polygon((150, 180, 200, 180, 250, 120, 230, 90, 130, 100),(0,0,0),(0,0,0))\n",
    "#     draw.ellipse((1500-20,300-20,1500+20,300+20), (255,0,0), (255,0,0))\n",
    "    return im\n",
    "im_show=Draw_map1()\n",
    "plt.imshow(im_show)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 87,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Draw_map2():\n",
    "    im = Image.new(\"RGB\", size=(2000,2000),color=(0,0,0)) \n",
    "    draw = ImageDraw.Draw(im,mode='RGB') \n",
    "    draw.rectangle((40,40,1960,1960),(255,255,255),(255,255,255))  \n",
    "    draw.rectangle((1500,1000,1600,1100),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((400,700,600,900),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((200,300,500,600),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((1500,500,1800,800),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((1200,1400,1600,1800),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((700,1200,960,1460),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((300,1600,500,1800),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((100,1100,300,1300),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((1100,250,1300,450),(0,0,0),(0,0,0)) \n",
    "    draw.polygon((900, 1070,1120, 1000,1150, 1100, 1100,1090,1050, 1200),(0,0,0),(0,0,0))\n",
    "    draw.pieslice((750, 1700, 950, 1900), 0,180,(0,0,0),(0,0,0))\n",
    "    draw.ellipse((900,550,1050,700),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((650,100,850,300),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((1700,130,1900,330),(0,0,0),(0,0,0))\n",
    "    draw.polygon((150, 180, 200, 180, 250, 120, 230, 90, 130, 100),(0,0,0),(0,0,0))\n",
    "    return im\n",
    "im_show=Draw_map2()\n",
    "plt.imshow(im_show)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 89,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Draw_map3():\n",
    "    im = Image.new(\"RGB\", size=(2000,2000),color=(0,0,0)) \n",
    "    draw = ImageDraw.Draw(im,mode='RGB') \n",
    "    draw.rectangle((40,40,1960,1960),(255,255,255),(255,255,255))  \n",
    "    draw.polygon((1150, 1180, 1200, 1180, 1250, 1120, 1230, 1090, 1130, 1100),(0,0,0),(0,0,0))\n",
    "    draw.polygon((400,450,600,750,500,800),(0,0,0),(0,0,0))\n",
    "    draw.rectangle((1500,750,1700,950),(0,0,0),(0,0,0))\n",
    "    draw.rectangle((300,1500,1000,1750),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((1450,1600,1600,1750),(0,0,0),(0,0,0))\n",
    "    draw.rectangle((1200,300,1400,500),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((500,1000,700,1200),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((750,900,700,1200),(0,0,0),(0,0,0))\n",
    "    return im\n",
    "im_show=Draw_map3()\n",
    "plt.imshow(im_show)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 45,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Function Scan is used to detect the distance between obstacles and robot.\n",
    "#Nearest(<50cm)||Near(50cm-130cm)||Medium(130cm-210cm)|Far(>210cm)\n",
    "#Furthest scanning distance is defined as 290cm\n",
    "def Scan(Current_x,Current_y,Angle,im):\n",
    "    Distance_level=0\n",
    "    Obstacle_distance=Robot_radium\n",
    "    Obstacle_distance_x=Current_x+Obstacle_distance*np.cos(Angle/180*np.pi)\n",
    "    Obstacle_distance_y=Current_y+Obstacle_distance*np.sin(Angle/180*np.pi)\n",
    "    while(im.getpixel((Obstacle_distance_x,Obstacle_distance_y))!=(0,0,0) and Obstacle_distance<250):#getpixiel obtains the degree of Gray Scale\n",
    "        Obstacle_distance+=5  #Search interval, can be changed\n",
    "        Obstacle_distance_x=Current_x+Obstacle_distance*np.cos(Angle/180*np.pi)\n",
    "        Obstacle_distance_y=Current_y+Obstacle_distance*np.sin(Angle/180*np.pi)\n",
    "    if 0<=Obstacle_distance<90:\n",
    "        Distance_level=0 #Nearear\n",
    "    elif 90<=Obstacle_distance<170:\n",
    "        Distance_level=1 #Near\n",
    "    elif 170<=Obstacle_distance<250:\n",
    "        Distance_level=2 #Medium\n",
    "    else:\n",
    "        Distance_level=3 #Far\n",
    "    return Distance_level"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 46,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Is_Crash(Current_x,Current_y,im):\n",
    "    Crash=False\n",
    "    Degree=[-150,-120,-90,-60,-30,0,30,60,90,120,150,180]\n",
    "    Distance=np.arange(0,50,5)\n",
    "    for i in Distance:\n",
    "        for j in Degree:\n",
    "            x=Current_x+i*np.cos(j/180*np.pi)\n",
    "            y=Current_y+i*np.sin(j/180*np.pi)\n",
    "            if im.getpixel((x,y))==(0,0,0):\n",
    "                Crash=True\n",
    "                break\n",
    "        if Crash==True:\n",
    "                break\n",
    "    return Crash"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 47,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Random_start(im):\n",
    "    Angle=np.random.random()*360\n",
    "    x,y=np.random.random(2)*2000\n",
    "    while(Is_Crash(x,y,im)==True):\n",
    "        x,y=np.random.random(2)*2000\n",
    "    return x,y,Angle"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 48,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Direction_min_level(Degree,Current_x,Current_y,Current_angle,im):\n",
    "    Level=[]\n",
    "    Degree=Degree+Current_angle\n",
    "    for i in Degree:\n",
    "        Level.append(Scan(Current_x,Current_y,i,im))\n",
    "    return min(Level)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 49,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Output_state_index(Left_min,Right_min,Up_min):\n",
    "    LRU=[]\n",
    "    LRU.append(Left_min)\n",
    "    LRU.append(Right_min)\n",
    "    LRU.append(Up_min)\n",
    "    return LRU[0]*16+LRU[1]*4+LRU[2]\n",
    "# print(Output_state_index(Left_min,Right_min,Up_min))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 50,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Choose_action is used to selection an action during the training process. It is based on the greedy strategy, if the random \n",
    "#chosen float(0-1) is inferior to current epsilon, robot choose random action to explore, if not, choose maximun Q value\n",
    "#action based on Q Table, more precisely based on the action-state range\n",
    "def Choose_action(Q_Table,Current_state,Action_times):\n",
    "    Epsilon=Epsilon_final+(Epsilon_start-Epsilon_final)*np.exp(-1*Decay_rate*Action_times)\n",
    "    State_action=Q_Table[Current_state,:]\n",
    "    if(np.random.random()<Epsilon or np.all(State_action==[0])):\n",
    "        Next_action=np.random.randint(Q_table1_actions_length)\n",
    "    else:\n",
    "        Next_action=np.argmax(State_action)\n",
    "    return Next_action"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 51,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Output_next_state(Current_x,Current_y,Current_angle,Action,im):\n",
    "    Reward=1\n",
    "    Crash=False\n",
    "    if Action==0:\n",
    "        Next_x=Current_x+Velocity_tripod*np.cos(Current_angle/180*np.pi)\n",
    "        Next_y=Current_y+Velocity_tripod*np.sin(Current_angle/180*np.pi)\n",
    "        Next_angle=Current_angle\n",
    "        if Is_Crash(Next_x,Next_y,im)==True:\n",
    "            Crash=True\n",
    "            Reward=-500\n",
    "        else:\n",
    "            Reward=2\n",
    "    elif Action==1:\n",
    "        Next_x=Current_x-Velocity_tripod*np.cos(Current_angle/180*np.pi)\n",
    "        Next_y=Current_y-Velocity_tripod*np.sin(Current_angle/180*np.pi)\n",
    "        Next_angle=Current_angle\n",
    "        if Is_Crash(Next_x,Next_y,im)==True:\n",
    "            Crash=True\n",
    "            Reward=-500\n",
    "    elif Action==2:\n",
    "        Next_x=Current_x\n",
    "        Next_y=Current_y\n",
    "        Next_angle=Current_angle-45\n",
    "    elif Action==3:\n",
    "        Next_x=Current_x\n",
    "        Next_y=Current_y\n",
    "        Next_angle=Current_angle+45        \n",
    "    return Next_x,Next_y,Next_angle,Reward,Crash"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 52,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Movement_plot(Vec_x,Vec_y):\n",
    "    im=Image.new(\"RGB\", size=(2000,2000),color=(0,0,0))\n",
    "    draw = ImageDraw.Draw(im,mode='RGB')\n",
    "    draw.rectangle((40,40,1960,1960),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((1500,1000,1650,1150),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((400,700,550,850),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((200,300,350,450),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((1500,500,1550,650),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((1200,1400,1350,1550),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((700,1200,850,1350),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((300,1600,450,1750),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((100,1100,250,1250),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((1100,250,1250,400),(0,0,0),(0,0,0)) \n",
    "    draw.polygon((900, 1070,1120, 1000,1150, 1100, 1100,1090,1050, 1200),(0,0,0),(0,0,0))\n",
    "    draw.pieslice((750, 1700, 900, 1850), 0,180,(0,0,0),(0,0,0))\n",
    "    draw.ellipse((900,550,1050,700),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((650,100,750,200),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((1700,130,1800,230),(0,0,0),(0,0,0))\n",
    "    draw.polygon((150, 180, 200, 180, 250, 120, 230, 90, 130, 100),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((Vec_x[0]-40,Vec_y[0]-40, Vec_x[0]+40,Vec_y[0]+40),(0,255,0),(0,255,0)) \n",
    "    for i,j in zip(Vec_x[1:],Vec_y[1:]):\n",
    "        draw.ellipse((i-40,j-40, i+40,j+40),(0,255,0),(0,255,0)) \n",
    "    return im"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 53,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "***********************************************************\n",
      "Succeed to initialize Q-Table!\n",
      "***********************************************************\n"
     ]
    }
   ],
   "source": [
    "Q_Table=Initial_Q_Table(Q_table1_actions_length,Q_table1_states_length)\n",
    "global Epoche\n",
    "Epoche=0\n",
    "global Action_times #Rigister the totality of the times of selecting actions, including the random selections and selection based on Q_Table\n",
    "Action_times=0\n",
    "global Epoche_action_interval\n",
    "Epoche_action_interval=[]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 54,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "Start x=149.251874  y=268.432879\n",
      "***********************************************************\n",
      "Epoche=0\n",
      "Epsilon=0.999994\n",
      "***********************************************************\n",
      "Epoche=10\n",
      "Epsilon=0.993919\n",
      "***********************************************************\n",
      "Epoche=20\n",
      "Epsilon=0.979915\n",
      "***********************************************************\n",
      "Epoche=30\n",
      "Epsilon=0.963360\n",
      "***********************************************************\n",
      "Epoche=40\n",
      "Epsilon=0.949025\n",
      "***********************************************************\n",
      "Epoche=50\n",
      "Epsilon=0.943323\n",
      "***********************************************************\n",
      "Epoche=60\n",
      "Epsilon=0.938272\n",
      "***********************************************************\n",
      "Epoche=70\n",
      "Epsilon=0.935330\n",
      "***********************************************************\n",
      "Epoche=80\n",
      "Epsilon=0.931880\n",
      "***********************************************************\n",
      "Epoche=90\n",
      "Epsilon=0.929213\n",
      "***********************************************************\n",
      "Epoche=100\n",
      "Epsilon=0.923923\n",
      "***********************************************************\n",
      "Epoche=110\n",
      "Epsilon=0.920330\n",
      "***********************************************************\n",
      "Epoche=120\n",
      "Epsilon=0.917431\n",
      "***********************************************************\n",
      "Epoche=130\n",
      "Epsilon=0.913278\n",
      "***********************************************************\n",
      "Epoche=140\n",
      "Epsilon=0.908382\n",
      "***********************************************************\n",
      "Epoche=150\n",
      "Epsilon=0.903953\n",
      "***********************************************************\n",
      "Epoche=160\n",
      "Epsilon=0.900195\n",
      "***********************************************************\n",
      "Epoche=170\n",
      "Epsilon=0.896486\n",
      "***********************************************************\n",
      "Epoche=180\n",
      "Epsilon=0.894198\n",
      "***********************************************************\n",
      "Epoche=190\n",
      "Epsilon=0.890984\n",
      "***********************************************************\n",
      "Epoche=200\n",
      "Epsilon=0.887556\n",
      "***********************************************************\n",
      "Epoche=210\n",
      "Epsilon=0.882831\n",
      "***********************************************************\n",
      "Epoche=220\n",
      "Epsilon=0.880700\n",
      "***********************************************************\n",
      "Epoche=230\n",
      "Epsilon=0.876802\n",
      "***********************************************************\n",
      "Epoche=240\n",
      "Epsilon=0.873903\n",
      "***********************************************************\n",
      "Epoche=250\n",
      "Epsilon=0.870806\n",
      "***********************************************************\n",
      "Epoche=260\n",
      "Epsilon=0.867216\n",
      "***********************************************************\n",
      "Epoche=270\n",
      "Epsilon=0.865875\n",
      "***********************************************************\n",
      "Epoche=280\n",
      "Epsilon=0.862622\n",
      "***********************************************************\n",
      "Epoche=290\n",
      "Epsilon=0.861053\n",
      "***********************************************************\n",
      "Epoche=300\n",
      "Epsilon=0.859361\n",
      "***********************************************************\n",
      "Epoche=310\n",
      "Epsilon=0.857060\n",
      "***********************************************************\n",
      "Epoche=320\n",
      "Epsilon=0.854649\n",
      "***********************************************************\n",
      "Epoche=330\n",
      "Epsilon=0.853411\n",
      "***********************************************************\n",
      "Epoche=340\n",
      "Epsilon=0.851311\n",
      "***********************************************************\n",
      "Epoche=350\n",
      "Epsilon=0.848805\n",
      "***********************************************************\n",
      "Epoche=360\n",
      "Epsilon=0.846784\n",
      "***********************************************************\n",
      "Epoche=370\n",
      "Epsilon=0.844820\n",
      "***********************************************************\n",
      "Epoche=380\n",
      "Epsilon=0.843469\n",
      "***********************************************************\n",
      "Epoche=390\n",
      "Epsilon=0.840895\n",
      "***********************************************************\n",
      "Epoche=400\n",
      "Epsilon=0.839384\n",
      "***********************************************************\n",
      "Epoche=410\n",
      "Epsilon=0.837272\n",
      "***********************************************************\n",
      "Epoche=420\n",
      "Epsilon=0.834627\n",
      "***********************************************************\n",
      "Epoche=430\n",
      "Epsilon=0.832698\n",
      "***********************************************************\n",
      "Epoche=440\n",
      "Epsilon=0.830268\n",
      "***********************************************************\n",
      "Epoche=450\n",
      "Epsilon=0.828586\n",
      "***********************************************************\n",
      "Epoche=460\n",
      "Epsilon=0.827388\n",
      "***********************************************************\n",
      "Epoche=470\n",
      "Epsilon=0.825771\n",
      "***********************************************************\n",
      "Epoche=480\n",
      "Epsilon=0.823917\n",
      "***********************************************************\n",
      "Epoche=490\n",
      "Epsilon=0.821279\n",
      "***********************************************************\n",
      "Epoche=500\n",
      "Epsilon=0.819586\n",
      "***********************************************************\n",
      "Epoche=510\n",
      "Epsilon=0.817926\n",
      "***********************************************************\n",
      "Epoche=520\n",
      "Epsilon=0.815696\n",
      "***********************************************************\n",
      "Epoche=530\n",
      "Epsilon=0.813751\n",
      "***********************************************************\n",
      "Epoche=540\n",
      "Epsilon=0.811242\n",
      "***********************************************************\n",
      "Epoche=550\n",
      "Epsilon=0.810131\n",
      "***********************************************************\n",
      "Epoche=560\n",
      "Epsilon=0.807465\n",
      "***********************************************************\n",
      "Epoche=570\n",
      "Epsilon=0.806494\n",
      "***********************************************************\n",
      "Epoche=580\n",
      "Epsilon=0.804556\n",
      "***********************************************************\n",
      "Epoche=590\n",
      "Epsilon=0.803221\n",
      "***********************************************************\n",
      "Epoche=600\n",
      "Epsilon=0.800558\n",
      "***********************************************************\n",
      "Epoche=610\n",
      "Epsilon=0.798921\n",
      "***********************************************************\n",
      "Epoche=620\n",
      "Epsilon=0.796883\n",
      "***********************************************************\n",
      "Epoche=630\n",
      "Epsilon=0.794506\n",
      "***********************************************************\n",
      "Epoche=640\n",
      "Epsilon=0.792172\n",
      "***********************************************************\n",
      "Epoche=650\n",
      "Epsilon=0.790952\n"
     ]
    },
    {
     "ename": "KeyboardInterrupt",
     "evalue": "",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mKeyboardInterrupt\u001b[0m                         Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-54-454d730b9ac3>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m()\u001b[0m\n\u001b[0;32m     69\u001b[0m     \u001b[1;32mreturn\u001b[0m \u001b[0mQ_Table\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m     70\u001b[0m \u001b[0mprint\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m---> 71\u001b[1;33m \u001b[0mQ_Table_Final\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mRun1\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m     72\u001b[0m \u001b[0mQ_Table_Final\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mpd\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mDataFrame\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mQ_Table_Final\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mcolumns\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mQ_table1_actions\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mindex\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mQ_table1_states\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m     73\u001b[0m \u001b[0mQ_Table_Final\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32m<ipython-input-54-454d730b9ac3>\u001b[0m in \u001b[0;36mRun1\u001b[1;34m()\u001b[0m\n\u001b[0;32m     21\u001b[0m     \u001b[1;32mwhile\u001b[0m \u001b[0mEpoche_false\u001b[0m\u001b[1;33m<\u001b[0m\u001b[1;36m5000\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m     22\u001b[0m         \u001b[0mCurrent_left_obstacle_level\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mDirection_min_level\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mLeft_degree\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_x\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_y\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_angle\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mim\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m---> 23\u001b[1;33m         \u001b[0mCurrent_right_obstacle_level\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mDirection_min_level\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mRight_degree\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_x\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_y\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_angle\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mim\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m     24\u001b[0m         \u001b[0mCurrent_up_obstacle_level\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mDirection_min_level\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mUp_degree\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_x\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_y\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_angle\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mim\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m     25\u001b[0m         \u001b[0mCurrent_state\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mOutput_state_index\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mCurrent_left_obstacle_level\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_right_obstacle_level\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_up_obstacle_level\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32m<ipython-input-48-ba6efb9753b3>\u001b[0m in \u001b[0;36mDirection_min_level\u001b[1;34m(Degree, Current_x, Current_y, Current_angle, im)\u001b[0m\n\u001b[0;32m      1\u001b[0m \u001b[1;32mdef\u001b[0m \u001b[0mDirection_min_level\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mDegree\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_x\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_y\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_angle\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mim\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      2\u001b[0m     \u001b[0mLevel\u001b[0m\u001b[1;33m=\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m----> 3\u001b[1;33m     \u001b[0mDegree\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mDegree\u001b[0m\u001b[1;33m+\u001b[0m\u001b[0mCurrent_angle\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m      4\u001b[0m     \u001b[1;32mfor\u001b[0m \u001b[0mi\u001b[0m \u001b[1;32min\u001b[0m \u001b[0mDegree\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      5\u001b[0m         \u001b[0mLevel\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mappend\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mScan\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mCurrent_x\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_y\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mi\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mim\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;31mKeyboardInterrupt\u001b[0m: "
     ]
    }
   ],
   "source": [
    "def Run1():\n",
    "    global Epoche\n",
    "    global Action_times\n",
    "    global Epoche_action_interval\n",
    "    Crash=False\n",
    "    Choose_map=np.random.randint(3)\n",
    "    if Choose_map==0:\n",
    "        im=Draw_map1()\n",
    "    elif Choose_map==1:\n",
    "        im=Draw_map2()\n",
    "    else:\n",
    "        im=Draw_map3()\n",
    "    Current_x,Current_y,Current_angle=Random_start(im)\n",
    "    print('Start x=%f  y=%f'%(Current_x,Current_y))\n",
    "    Epoche_false=0\n",
    "    Vec_x=[]\n",
    "    Vec_y=[]\n",
    "    Vec_x.append(Current_x)\n",
    "    Vec_y.append(Current_y)\n",
    "    Epoche_action_times=0\n",
    "    while Epoche_false<5000:\n",
    "        Current_left_obstacle_level=Direction_min_level(Left_degree,Current_x,Current_y,Current_angle,im)\n",
    "        Current_right_obstacle_level=Direction_min_level(Right_degree,Current_x,Current_y,Current_angle,im)\n",
    "        Current_up_obstacle_level=Direction_min_level(Up_degree,Current_x,Current_y,Current_angle,im)\n",
    "        Current_state=Output_state_index(Current_left_obstacle_level,Current_right_obstacle_level,Current_up_obstacle_level)\n",
    "        \n",
    "        Next_action=Choose_action(Q_Table,Current_state,Action_times)\n",
    "#         print('Current_x=%f  Current_y=%f  Current_angle=%f Current_action=%d'%(Current_x,Current_y,Current_angle,Next_action))\n",
    "        Next_x,Next_y,Next_angle,Reward,Crash=Output_next_state(Current_x,Current_y,Current_angle,Next_action,im)\n",
    "        \n",
    "        if Crash==True:\n",
    "            Q_target=Reward\n",
    "            if(Epoche%10==0):\n",
    "                print('***********************************************************')\n",
    "                print('Epoche=%d'%Epoche)\n",
    "                print(\"Epsilon=%f\"%(Epsilon_final+(Epsilon_start-Epsilon_final)*np.exp(-1*Decay_rate*Action_times)))\n",
    "                Movement_plot(Vec_x,Vec_y)\n",
    "                Vec_x=[]\n",
    "                Vec_y=[]\n",
    "            Epoche+=1\n",
    "            Epoche_false+=1\n",
    "            Choose_map=np.random.randint(3)\n",
    "            if Choose_map==0:\n",
    "                im=Draw_map1()\n",
    "            elif Choose_map==1:\n",
    "                im=Draw_map2()\n",
    "            else:\n",
    "                im=Draw_map3()\n",
    "            Next_x,Next_y,Next_angle=Random_start(im)\n",
    "            Epoche_action_interval.append(Epoche_action_times)\n",
    "            Epoche_action_times=0\n",
    "        \n",
    "        else:\n",
    "            Next_left_obstacle_level=Direction_min_level(Left_degree,Next_x,Next_y,Next_angle,im)\n",
    "            Next_right_obstacle_level=Direction_min_level(Right_degree,Next_x,Next_y,Next_angle,im)\n",
    "            Next_up_obstacle_level=Direction_min_level(Up_degree,Next_x,Next_y,Next_angle,im)\n",
    "            Next_state=Output_state_index(Next_left_obstacle_level,Next_right_obstacle_level,Next_up_obstacle_level)\n",
    "            Q_target=Reward+Beta*max(Q_Table[Next_state])\n",
    "            \n",
    "        Q_Table[Current_state][Next_action]+=Alpha*(Q_target-Q_Table[Current_state][Next_action])\n",
    "        Current_x=Next_x\n",
    "        Current_y=Next_y\n",
    "        Current_a=Next_angle\n",
    "        Vec_x.append(Current_x)\n",
    "        Vec_y.append(Current_y)\n",
    "        Action_times+=1\n",
    "        Epoche_action_times+=1\n",
    "    plt.plot(np.arange(len(Epoche_action_interval)),Epoche_action_interval)\n",
    "    return Q_Table\n",
    "print()\n",
    "Q_Table_Final=Run1()\n",
    "Q_Table_Final=pd.DataFrame(Q_Table_Final,columns=Q_table1_actions,index=Q_table1_states)\n",
    "Q_Table_Final\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "def test(Q_Table):\n",
    "    Crash=False\n",
    "    im=Draw_map1()\n",
    "    Current_x,Current_y,Current_angle=Random_start(im)\n",
    "    Test_time=0\n",
    "    Vec_x=[]\n",
    "    Vec_y=[]\n",
    "    while Test_time<100001:\n",
    "        Current_left_obstacle_level=Direction_min_level(Left_degree,Current_x,Current_y,Current_angle,im)\n",
    "        Current_right_obstacle_level=Direction_min_level(Right_degree,Current_x,Current_y,Current_angle,im)\n",
    "        Current_up_obstacle_level=Direction_min_level(Up_degree,Current_x,Current_y,Current_angle,im)\n",
    "        Current_state=Output_state_index(Current_left_obstacle_level,Current_right_obstacle_level,Current_up_obstacle_level)\n",
    "        Next_action=np.argmax(Q_Table[Current_state])\n",
    "#         print(\"------------------------------------------------------------------------------------------\")\n",
    "#         print('Current_x=%f   Current_y=%f   Current_angle=%f'%(Current_x,Current_y,(Current_angle%360)))\n",
    "        Next_x,Next_y,Next_angle,Reward,Crash=Output_next_state(Current_x,Current_y,Current_angle,Next_action,im)\n",
    "#         print('Current_state=%d'%Current_state)\n",
    "#         print('Next_action=%d'%Next_action)\n",
    "#         print('Next_x=%f   Next_y=%f   Next_angle=%f'%(Next_x,Next_y,(Next_angle%360)))\n",
    "        \n",
    "        Vec_x.append(Current_x)\n",
    "        Vec_y.append(Current_y)\n",
    "        if Crash==True:\n",
    "            print('Boom')\n",
    "            print(Test_time)\n",
    "            break\n",
    "        else:\n",
    "            if(Test_time%10000==0):\n",
    "                print(\"******************\")\n",
    "                print(Test_time)\n",
    "                im_show=Movement_plot(Vec_x,Vec_y)\n",
    "                print(\"******************\")\n",
    "                plt.imshow(im_show)\n",
    "                plt.show()\n",
    "                Vec_x=[]\n",
    "                Vec_y=[]\n",
    "                Next_x,Next_y,Current_a=Random_start(im)\n",
    "        Current_x=Next_x\n",
    "        Current_y=Next_y\n",
    "        Current_angle=Next_angle\n",
    "        Test_time+=1\n",
    "test(Q_Table_Final.as_matrix())\n",
    "# np.savetxt(\"Q_Table_try.txt\",Q_Table_Final.as_matrix())\n",
    "            "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Q_Table_Final.iloc[30,:]"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
